﻿using System;
using System.IO;
using System.Text;
using System.Drawing;
using System.IO.Ports;
using System.Windows.Forms;
using System.Drawing.Imaging;
using System.Collections.Generic;

using AForge;
using AForge.Math;
using AForge.Video;
using AForge.Imaging;
using AForge.Robotics.Lego;
using AForge.Imaging.Filters;
using AForge.Video.DirectShow;

using Emgu.CV;
using Emgu.Util;
using Emgu.CV.CvEnum;
using Emgu.CV.Structure;

using CokAracliKontrol.PathFinding;
using CokAracliKontrol.VehicleControl;

namespace CokAracliKontrol
{
    public partial class MainForm : Form
    {
        // Defining a path finder for system
        Pathfinder _pathfinder = new Pathfinder();
       
        //Definition of the list of lego driver for each vehichle 
        private List<LegoDriver> vehicleDrivers;

        // Flag for camera status
        private bool onAir = false;

        // Flag for bluetooth connection
        private bool closeConnection = false;
        
        // Flag for successfully calculated paths
        private bool pathCalculated = false;

        // Flag for running simulation task
        private bool runSimulation = false;

        // Flag for real task status
        private bool IsTaskStarted = false;

        // Flag for sorted vehicles
        private bool IsSorted = false;

        //private bool errorMessageShowed = false;
        //public int xold, yold;

        // Minimum dimensions for blobs
        public int minBlobWidth, minBlobHeight;

        // Obstacle color with default value
        public string obstacleColor = "143-250|0-115|0-29";  // wood obstacle

        // Precision of reaching the target coordinate
        public int breakRadius = 1;

        // Motor port and power definitions
        public string leftMotor, rightMotor;
        public int leftMotorPower, rightMotorPower;

        // Defining camera capture object
        public Capture _capture;
        
        // Defining main image which is going to show when live capture is wanted
        public Image<Bgr, Byte> motherImage;

        // Defining color filter
        ColorFiltering colorFilter = new ColorFiltering();

        // Defining gray filter, because AForge.NET's blob detection is working with only gray scale images.
        // (This is not really needed for blob detection, we can just need any kind of binary image)
        GrayscaleBT709 grayFilter = new GrayscaleBT709();

        // Defining blob
        BlobCounter blobCounter = new BlobCounter();

        //Dictionary<int, Point> initialPositions = new Dictionary<int, Point>();

        // Path list for every vehicle
        Dictionary<int, List<Point>> paths = new Dictionary<int, List<Point>>();

        // Movement list which is derived from path list for every vehicle
        Dictionary<int, List<Point>> moves = new Dictionary<int, List<Point>>();
        Dictionary<int, int> vehicleStepCounters = new Dictionary<int, int>();

        // These lists store vehicle id's according to their priorites which is set by user; path length or battery level.
        List<int> sortedVehicleIDs = new List<int>();
        List<int> sortedVehicleCriteria = new List<int>();

        // Because in windows, with the using webcam and EmguCV (an OpenCV wrapper) can not get directly 640x480 frames
        // (In windows server 2008 this is not being any problem, but in windows xp it does.)
        Image<Bgr, Byte> CaptureFrame()
        {
            Image<Bgr, Byte> frame;
            frame = _capture.QueryFrame();
            return frame.Resize(640, 480);
        }

        // Adjust images width according to board control's grid size
        int GetColumnPixels(Bitmap img)
        {
            return img.Width / Pathfinder.gridSizeX;
        }

        // Adjust images height according to board control's grid size
        int GetRowPixels(Bitmap img)
        {
            return img.Height / Pathfinder.gridSizeY;
        }

        private bool IsBetween(int n, int limit1, int limit2)
        {
            return ((n >= limit1) && (n <= limit2)) || ((n >= limit2) && (n <= limit1));
        }

        private bool IsBetween(double n, double limit1, double limit2)
        {
            return ((n >= limit1) && (n <= limit2)) || ((n >= limit2) && (n <= limit1));
        }

        private static bool IsNumeric(object myValue)
        {
            bool isNum;
            int retNum;

            if (myValue != null)
            {
                isNum = Int32.TryParse(Convert.ToString(myValue), System.Globalization.NumberStyles.Any, System.Globalization.NumberFormatInfo.InvariantInfo, out retNum);
            }
            else
            {
                isNum = false;
            }

            return isNum;
        }

        private bool validateGrid(ComboGrid.ComboGrid grid, int row)
        {
            bool result = false;

            string vid = grid.CellAt(row, "Vehicle ID").Text; //grid.Cells[row, 0].Text;
            //string ic = grid.Cells[row, 2].Text;
            string cc = grid.CellAt(row, "Current Coordinate").Text;
            string tc = grid.CellAt(row, "Target Coordinate").Text;

            if (vid != null && vid != "")
            {
                if (Convert.ToInt32(vid) >= 0)
                {
                    result = true;
                }
                else
                {
                    result = false;
                }

            }
            else
            {
                result = false;
            }

            if (cc != null && cc != "")
            {
                if (cc.Contains(","))
                {
                    string[] parts = cc.Split(',');

                    if (IsNumeric(parts[0]) && IsNumeric(parts[1]))
                    {
                        if (Convert.ToInt32(parts[0]) < Pathfinder.gridSizeX
                             && Convert.ToInt32(parts[1]) < Pathfinder.gridSizeY)
                        {
                            result = true;
                        }
                        else
                        {
                            result = false;
                        }
                    }
                    else
                    {
                        result = false;
                    }
                }
                else
                {
                    result = false;
                }
            }
            else
            {
                result = false;
            }

            if (tc != null && tc != "")
            {
                if (tc.Contains(","))
                {
                    string[] parts = tc.Split(',');
                    if (IsNumeric(parts[0]) && IsNumeric(parts[1]))
                    {
                        if (Convert.ToInt32(parts[0]) < Pathfinder.gridSizeX
                             && Convert.ToInt32(parts[1]) < Pathfinder.gridSizeY)
                        {
                            result = true;
                        }
                        else
                        {
                            result = false;
                        }
                    }
                    else
                    {
                        result = false;
                    }
                }
                else
                {
                    result = false;
                }
            }
            else
            {
                result = false;
            }

            return result;
        }

        /*
        private double GetProximityLevel(int xold, int yold, int xnew, int ynew)
        {
            return Math.Sqrt(Math.Pow(xnew - xold, 2) + Math.Pow(ynew - yold, 2));
        }
        */

        // Recalculate the vehicle's path
        private void Recalculate(int vehicleID)
        {
            _pathfinder.ClearLogic();
            _pathfinder.Pathfind(vehicleID);
            _pathfinder.HighlightPath(vehicleID);

            if (validateGrid(comboGridVehicle, vehicleID - 1))
            {
                //if (tbObstacleColor.Text == null || tbObstacleColor.Text == "")
                //{
                //    // Default color for obstacle
                //    tbObstacleColor.Text = "0-55|0-55|0-55";
                //}
                
                int priority;
                comboGridVehicle.CellAt(vehicleID - 1, "Path Length").Items.Clear();

                if (_pathfinder.HighlightPath(vehicleID) != null)
                {
                    priority = _pathfinder.HighlightPath(vehicleID).Count;
                }
                else
                {
                    priority = 0;
                }

                comboGridVehicle.CellAt(vehicleID - 1, "Path Length").Items.Add(priority.ToString());
                comboGridVehicle.CellAt(vehicleID - 1, "Path Length").SelectedIndex = 0;

                _pathfinder.DrawBoard(boardControl1, vehicleID, comboGridVehicle, obstacleColor);

                if (!timerMoveAgents.Enabled)
                {
                    // Refresh the form control
                    boardControl1.Invalidate();
                }
            }
        }

        // Fills the calculated path and move list and can show results on the form
        private void FillCalculatedPath(int vehicleID, bool showResults)
        {
            if (showResults)
            {
                Recalculate(vehicleID);
            }

            List<Point> path = _pathfinder.HighlightPath(vehicleID);
            paths[vehicleID] = new List<Point>();

            try
            {
                foreach (Point p in path)
                {
                    if (showResults)
                    {
                        lbPathList.Items.Add("Vehicle #" + vehicleID.ToString() + ": " + p.ToString());
                    }
                    paths[vehicleID].Add(p);
                }

                moves[vehicleID] = new List<Point>();

                for (int i = 1; i < path.Count; i++)
                {
                    Point movement;
                    movement = Pathfinder.ExtractMovement(path[i - 1], path[i]);
                    moves[vehicleID].Add(movement);

                    if (showResults)
                    {
                        lbMotionList.Items.Add("Vehicle #" + vehicleID.ToString() + ": " + movement.ToString());
                    }
                }

                vehicleStepCounters[vehicleID] = 0;
            }
            catch (Exception ex)
            {
                statusLabel.Text = ex.Message;
            }
        }

        // Seperates RGB range of the given color
        private void ParseColorRanges(string s, IntRange redRange, IntRange greenRange, IntRange blueRange)
        {
            string sred, sblue, sgreen;
            string[] parts;
            parts = s.Split('|');

            if (parts.Length != 3)
            {
                redRange.Min = 0;
                redRange.Max = 255;
                greenRange.Min = 0;
                greenRange.Max = 255;
                blueRange.Min = 0;
                blueRange.Max = 255;
                return;
            }


            sred = parts[0];
            sgreen = parts[1];
            sblue = parts[2];


            parts = sred.Split('-');
            redRange.Min = Convert.ToInt32(parts[0]);
            redRange.Max = Convert.ToInt32(parts[1]);


            parts = sgreen.Split('-');
            greenRange.Min = Convert.ToInt32(parts[0]);
            greenRange.Max = Convert.ToInt32(parts[1]);


            parts = sblue.Split('-');
            blueRange.Min = Convert.ToInt32(parts[0]);
            blueRange.Max = Convert.ToInt32(parts[1]);
        }

        // Replaced the coordinate of the vehicle in the simulation mode
        private void ModifyInitialPath(int vehicleID, int xx, int yy)
        {
            for (int i = 0; i < _pathfinder.Squares.GetLength(0); i++)
            {
                for (int j = 0; j < _pathfinder.Squares.GetLength(1); j++)
                {
                    CompleteSquare mysq;
                    mysq = _pathfinder.Squares[i, j];
                    if (mysq.ContentCode == SquareContent.BaseStart + vehicleID)
                    {
                        mysq.ContentCode = SquareContent.FreePath;
                    }
                }
            }
            _pathfinder.Squares[xx, yy].ContentCode = SquareContent.BaseStart + vehicleID;
        }

        // Gets the position of the vehicle
        private void GetVehiclePosition(Bitmap capturedImage, int vehicleID, ref int x, ref int y, ref double angle)
        {
            IntRange red, green, blue;

            red = new IntRange(0, 0);
            green = new IntRange(0, 0);
            blue = new IntRange(0, 0);

            string colorData = comboGridVehicle.CellAt(vehicleID - 1, "Vehicle Pattern").Text;

            ParseColorRanges(colorData, red, green, blue);

            ColorFiltering vehicleFilter = new ColorFiltering(red, green, blue);

            /*
            ShowFilteredImage f;
            f = new ShowFilteredImage();
            f.pictureBox1.Image = capturedImage;
            f.Text = "Filtreden Once";
            timerMoveAgents.Enabled = false;
            f.ShowDialog();
            timerMoveAgents.Enabled = true;
            */

            // Filter the image by given color
            vehicleFilter.ApplyInPlace(capturedImage);

            /*
            f = new ShowFilteredImage();
            f.pictureBox1.Image = capturedImage;
            f.Text = "Filtreden Sonra";
            timerMoveAgents.Enabled = false;
            f.ShowDialog();
            timerMoveAgents.Enabled = true;
            */

            //lock image for further processing 
            BitmapData objectData = capturedImage.LockBits(
                new Rectangle(0, 0, capturedImage.Width, capturedImage.Height),
                ImageLockMode.ReadOnly, capturedImage.PixelFormat);
            
            // Convert to filtered image to gray scaled one
            UnmanagedImage grayImage = grayFilter.Apply(new UnmanagedImage(objectData));

            // Unlock image
            capturedImage.UnlockBits(objectData);

            // Blob analysis
            blobCounter.MinWidth = minBlobWidth;
            blobCounter.MinHeight = minBlobHeight;
            blobCounter.ProcessImage(grayImage);
            Rectangle[] rects = blobCounter.GetObjectsRectangles();

            x = -1;
            y = -1;
            angle = -1;

            // Triangular color pattern
            if (rects.Length >= 3)
            {
                // Get the biggest 3 blob and process them as front and two rear marker
                VehicleRoof roof = new VehicleRoof(rects[0], rects[1], rects[2]);
                if (roof.Process())
                {
                    // Get the center of the triangular as the vehicle coordinate
                    x = roof.center.X / (grayImage.Width / Pathfinder.gridSizeX);
                    y = roof.center.Y / (grayImage.Height / Pathfinder.gridSizeY);

                    // Get the angle between the height from top to base of the triangular and the pixel coordinate
                    angle = roof.AngleAsDegrees;
                }
            }

            /*
            //Single color pattern
            if (rects.Length > 0)
            {
                vehicleArea = rects[0];

                // Get blob's center coordinates relative to image center 
                int centerX, centerY;
                centerX = (vehicleArea.Left + vehicleArea.Right) / 2;
                centerY = (vehicleArea.Top + vehicleArea.Bottom) / 2;

                x = centerX / (grayImage.Width / Pathfinder.gridSizeX);
                y = centerY / (grayImage.Height / Pathfinder.gridSizeY);
            }
            else
            {
                x = -1;
                y = -1;
                //statusLabel.Text = "No blobs are detected !";
            }
            */
        }

        // Detects and mark the obstacle on the map
        void DetectObstacle(Bitmap image, IntRange red, IntRange green, IntRange blue)
        {
            int rowpix = GetRowPixels(image);
            int colpix = GetColumnPixels(image);

            //GaussianBlur imgGaussianBlur = new GaussianBlur(colpix / 2, colpix);
            GaussianBlur imgGaussianBlur = new GaussianBlur(1, 1);
            imgGaussianBlur.ApplyInPlace(image);

            Dilatation dilFilter = new Dilatation();
            dilFilter.ApplyInPlace(image);

            for (int y = 0; y < image.Height; y++)
            {
                for (int x = 0; x < image.Width; x++)
                {
                    Color px;
                    px = image.GetPixel(x, y);

                    if (IsBetween(px.R, red.Min, red.Max) &&
                       IsBetween(px.G, green.Min, green.Max) &&
                       IsBetween(px.B, blue.Min, blue.Max))
                    {
                        double rowf = (double)y / rowpix;
                        double colf = (double)x / colpix;

                        int row = (int)Math.Round(rowf);
                        int col = (int)Math.Round(colf);

                        if (row >= Pathfinder.gridSizeX)
                        {
                            row = Pathfinder.gridSizeX - 1;
                        }

                        if (col >= Pathfinder.gridSizeY)
                        {
                            col = Pathfinder.gridSizeY - 1;
                        }

                        _pathfinder.Squares[col,row].ContentCode = SquareContent.Obstacle;
                    }
                }
            }
        }

        private void TurnVehicle(int vehicleID, bool clockwise)
        {
            if (clockwise)
            {
                vehicleDrivers[vehicleID - 1].SetMotors(leftMotor, rightMotor, leftMotorPower, -rightMotorPower, 100, 0);
            }
            else
            {
                vehicleDrivers[vehicleID - 1].SetMotors(leftMotor, rightMotor, -leftMotorPower, rightMotorPower, 100, 0);
            }
        }

        private void MarchVehicle(int vehicleID)
        {
            vehicleDrivers[vehicleID - 1].SetMotors(leftMotor, rightMotor, leftMotorPower, rightMotorPower, 50, 0);
        }

        private void StopVehicle(int vehicleID)
        {
            vehicleDrivers[vehicleID - 1].SetMotors(leftMotor, rightMotor, 0, 0, 0, 0);
        }

        private void ControlVehicle(int vehicleID)
        {
            double angle;
            angle = Convert.ToDouble(comboGridVehicle.CellAt(vehicleID - 1, "Orientation Angle").Text);
            List<Point> destiny;

            destiny = moves[vehicleID];

            int x1, y1, x2, y2;
            string posstr;
            posstr = comboGridVehicle.CellAt(vehicleID - 1, "Current Coordinate").Text;
            string[] parts;
            parts = posstr.Split(',');

            x1 = Convert.ToInt32(parts[0]);
            y1 = Convert.ToInt32(parts[1]);

            posstr = comboGridVehicle.CellAt(vehicleID - 1, "Target Coordinate").Text;
            parts = posstr.Split(',');

            x2 = Convert.ToInt32(parts[0]);
            y2 = Convert.ToInt32(parts[1]);

            int dx, dy;
            dx = Math.Abs(x1 - x2);
            dy = Math.Abs(y1 - y2);

            if (destiny == null || destiny.Count == 0 || (dx < breakRadius || dy < breakRadius)) // (x1 == x2 && y1 == y2)
            {
                StopVehicle(vehicleID);

                if (comboGridVehicle.CellAt(vehicleID - 1, "Task Status").Items.Count == 0)
                {
                    comboGridVehicle.CellAt(vehicleID - 1, "Task Status").BackColor = Color.Green;
                    comboGridVehicle.CellAt(vehicleID - 1, "Task Status").ForeColor = Color.White;
                    comboGridVehicle.CellAt(vehicleID - 1, "Task Status").Items.Add("Done !");
                    comboGridVehicle.CellAt(vehicleID - 1, "Task Status").SelectedIndex = 0;
                }

                return;
            }

            Point move;
            move = destiny[0];

            const double angleRadius = 15;

            if (move.Equals(new Point(1, 0)))
            {
                // right
                if (IsBetween(angle, 90 - angleRadius, 90 + angleRadius))
                {
                    MarchVehicle(vehicleID);
                }
                else
                {
                    if (IsBetween(angle, 0, 90) || IsBetween(angle, 270, 360))
                    {
                        TurnVehicle(vehicleID, true);
                    }
                    else if (IsBetween(angle, 90, 270))
                    {
                        TurnVehicle(vehicleID, false);
                    }
                }
            }

            if (move.Equals(new Point(-1, 0)))
            {
                // left
                if (IsBetween(angle, 270 - angleRadius, 270 + angleRadius))
                {
                    MarchVehicle(vehicleID);
                }
                else
                {
                    if (IsBetween(angle, 0, 90) || IsBetween(angle, 270, 360))
                    {
                        TurnVehicle(vehicleID, false);
                    }
                    else if (IsBetween(angle, 90, 270))
                    {
                        TurnVehicle(vehicleID, true);
                    }
                }
            }

            if (move.Equals(new Point(0, -1)))
            {
                // up
                if (IsBetween(angle, 0, angleRadius) || IsBetween(angle, 360 - angleRadius, 360))
                {
                    MarchVehicle(vehicleID);
                }
                else
                {
                    if (IsBetween(angle, 180, 360))
                    {
                        TurnVehicle(vehicleID, true);
                    }
                    else if (IsBetween(angle, 0, 180))
                    {
                        TurnVehicle(vehicleID, false);
                    }
                }
            }

            if (move.Equals(new Point(0, 1)))
            {
                // down
                if (IsBetween(angle, 180 - angleRadius, 180 + angleRadius))
                {
                    MarchVehicle(vehicleID);
                }
                else
                {
                    if (IsBetween(angle, 180, 360))
                    {
                        TurnVehicle(vehicleID, false);
                    }
                    else if (IsBetween(angle, 0, 180))
                    {
                        TurnVehicle(vehicleID, true);
                    }
                }
            }

            if (move.Equals(new Point(1, 1)))
            {
                // down right
                if (IsBetween(angle, 135 - angleRadius, 135 + angleRadius))
                {
                    MarchVehicle(vehicleID);
                }
                else
                {
                    if (IsBetween(angle, 0, 135) || IsBetween(angle, 315, 360))
                    {
                        TurnVehicle(vehicleID, true);
                    }
                    else if (IsBetween(angle, 135, 315))
                    {
                        TurnVehicle(vehicleID, false);
                    }
                }
            }

            if (move.Equals(new Point(-1, 1)))
            {
                // down left
                if (IsBetween(angle, 225 - angleRadius, 225 + angleRadius))
                {
                    MarchVehicle(vehicleID);
                }
                else
                {
                    if (IsBetween(angle, 0, 45) || IsBetween(angle, 225, 360))
                    {
                        TurnVehicle(vehicleID, false);
                    }
                    else if (IsBetween(angle, 45, 225))
                    {
                        TurnVehicle(vehicleID, true);
                    }
                }
            }

            if (move.Equals(new Point(1, -1)))
            {
                // up right
                if (IsBetween(angle, 45 - angleRadius, 45 + angleRadius))
                {
                    MarchVehicle(vehicleID);
                }
                else
                {
                    if (IsBetween(angle, 0, 45) || IsBetween(angle, 225, 360))
                    {
                        TurnVehicle(vehicleID, true);
                    }
                    else if (IsBetween(angle, 45, 225))
                    {
                        TurnVehicle(vehicleID, false);
                    }
                }
            }

            if (move.Equals(new Point(-1, -1)))
            {
                // up right
                if (IsBetween(angle, 315 - angleRadius, 315 + angleRadius))
                {
                    MarchVehicle(vehicleID);
                }
                else
                {
                    if (IsBetween(angle, 0, 135) || IsBetween(angle, 315, 360))
                    {
                        TurnVehicle(vehicleID, false);
                    }
                    else if (IsBetween(angle, 135, 315))
                    {
                        TurnVehicle(vehicleID, true);
                    }
                }
            }
        }

        public MainForm()
        {

            //SplashScreen.SplashScreen.Status = "Çok Silahşorlar başlatılıyor ...";
            //System.Threading.Thread.Sleep(1500);

            InitializeComponent();

            //SplashScreen.SplashScreen.Status = "Donanım ayarları yükleniyor ...";
            //System.Threading.Thread.Sleep(2500);

            //SplashScreen.SplashScreen.Status = "Çok Silahşorlar açılıyor.";
            //System.Threading.Thread.Sleep(1000);

            //SplashScreen.SplashScreen.Close();
        }

        private void MainForm_Load(object sender, EventArgs e)
        {
            toolTip1.SetToolTip(btnConnect, "... Connect vehicles and get Current Coordinates");
            toolTip1.SetToolTip(btnObstacle, "... Detect obstacle \n Current Pattern:" + obstacleColor);
            toolTip1.SetToolTip(btnCalculate, "... Calculate all paths");
            toolTip1.SetToolTip(btnStartTask, "... Go go go !");
            toolTip1.SetToolTip(btnStartSimulationTask, "... Simulate task");
            toolTip1.SetToolTip(btnShowPMList, "... Show Position and Motion list of vehicles");

            try
            {
                for (int i = 0; i < comboGridVehicle.RowsCount; i++)
                {
                    comboGridVehicle.CellAt(i, "Connection Port").Items.AddRange(SerialPort.GetPortNames());
                    comboGridVehicle.CellAt(i, "Vehicle ID").Items.Add((i + 1).ToString());
                    comboGridVehicle.CellAt(i, "Vehicle ID").SelectedIndex = 0;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }

            //for (int i = 0; i < comboGridVehicle.RowsCount - 1; i++)
            //{
            //    comboGridVehicle.CellAt(i, "Vehicle Name").Items.Add("TEST " + i.ToString());
            //    comboGridVehicle.CellAt(i, "Vehicle Name").SelectedIndex = 0;
            //}
            
            foreach (ComboBox cell in comboGridVehicle.CellsOfColumn("Vehicle ID"))
            {
                cell.Enabled = false;
                cell.DropDownStyle = ComboBoxStyle.DropDownList;
            }

            foreach (ComboBox cell in comboGridVehicle.CellsOfColumn("Vehicle Name"))
            {
                cell.DropDownStyle = ComboBoxStyle.DropDownList;
            }

            foreach (ComboBox cell in comboGridVehicle.CellsOfColumn("Task Status"))
            {
                cell.DropDownStyle = ComboBoxStyle.DropDownList;
            }

            foreach (ComboBox cell in comboGridVehicle.CellsOfColumn("Battery (%)"))
            {
                cell.DropDownStyle = ComboBoxStyle.DropDownList;
            }

            foreach (ComboBox cell in comboGridVehicle.CellsOfColumn("Path Length"))
            {
                cell.DropDownStyle = ComboBoxStyle.DropDownList;
            }

            foreach (ComboBox cell in comboGridVehicle.CellsOfColumn("Connection Port"))
            {
                cell.DropDownStyle = ComboBoxStyle.DropDownList;
            }

            foreach (ComboBox cell in comboGridVehicle.CellsOfColumn("Vehicle Pattern"))
            {
                cell.Items.Add("Select...");
                cell.Items.Add("139-255|100-240|0-73"); // yellow pattern
                cell.Items.Add("0-76|100-240|115-255"); // blue pattern
                cell.SelectedIndexChanged += new EventHandler(comboGridCell_Handler);
            }

            colorFilter.Red = new IntRange(163, 255);
            colorFilter.Green = new IntRange(211, 255);
            colorFilter.Blue = new IntRange(0, 187);

            blobCounter.MinWidth = minBlobWidth;
            blobCounter.MinHeight = minBlobHeight;
            blobCounter.FilterBlobs = true;
            blobCounter.ObjectsOrder = ObjectsOrder.Size;

            leftMotor = "Port B";
            rightMotor = "Port C";

            leftMotorPower = 65;
            rightMotorPower = 65;
        }

        private void boardControl1_MouseMoveSpecial(object sender, EventArgs e)
        {
            Point point = boardControl1.Pos;

            if (point.X == -1 || point.Y == -1)
            {
                return;
            }

            int count = _pathfinder.Squares[point.X, point.Y].DistanceSteps;

            if (count >= Pathfinder.bigNumber)
            {
                statusLabel.Text = "No possible path.";
            }
            else if (count == 1)
            {
                statusLabel.Text = "1 step";
            }
            else if (count == 0)
            {
                statusLabel.Text = "Current";
            }
            else
            {
                statusLabel.Text = count.ToString() + " steps.";
            }

            statusLabel.Text += " X:" + point.X.ToString() + " Y:" + point.Y.ToString();
        }

        private void boardControl1_MouseClick(object sender, MouseEventArgs e)
        {
            Point point = boardControl1.Pos;

            int selectedRow = comboGridVehicle.SelectedElement.Y;

            if (point.X == -1 || point.Y == -1 || selectedRow < 0)
            {
                return;
            }

            int vehicleID = Convert.ToInt32(comboGridVehicle.CellAt(selectedRow, "Vehicle ID").Text);

            if (e.Button == MouseButtons.Left)
            {
                for (int row = 0; row < Pathfinder.gridSizeY; row++)
                {
                    for (int col = 0; col < Pathfinder.gridSizeX; col++)
                    {
                        if (_pathfinder.Squares[col, row].ContentCode == SquareContent.FinishLocation(vehicleID))
                        {
                            _pathfinder.Squares[col, row].ContentCode = SquareContent.FreePath;
                            break;
                        }
                    }
                }

                if (_pathfinder.Squares[point.X, point.Y].ContentCode != SquareContent.Obstacle)
                {
                    _pathfinder.Squares[point.X, point.Y].ContentCode = SquareContent.FinishLocation(vehicleID);
                }

                comboGridVehicle.CellAt(selectedRow, "Target Coordinate").Text = point.X.ToString() + "," + point.Y.ToString();

                //Recalculate(vehicleID);
            }

            if (e.Button == MouseButtons.Right)
            {
                if (_pathfinder.Squares[point.X, point.Y].ContentCode == SquareContent.FreePath)
                {
                    _pathfinder.Squares[point.X, point.Y].ContentCode = SquareContent.Obstacle;
                    //Recalculate(vehicleID);
                }
                else if (_pathfinder.Squares[point.X, point.Y].ContentCode == SquareContent.Obstacle)
                {
                    _pathfinder.Squares[point.X, point.Y].ContentCode = SquareContent.FreePath;
                    //Recalculate(vehicleID);
                }
            }

            Recalculate(vehicleID);
        }

        private void btnCalculate_Click(object sender, EventArgs e)
        {
            int errorCount = 0;
            pathCalculated = false;

            for (int v = 0; v < comboGridVehicle.RowsCount; v++)
            {
                if (!validateGrid(comboGridVehicle, v))
                {
                    errorCount++;
                    continue;
                }

                int vehicleID = Convert.ToInt32(comboGridVehicle.CellAt(v, "Vehicle ID").Text);

                //string initialCoordinate = comboGridVehicle.CellAt(v, "Current Coordinate").Text;
                //string[] iCor = initialCoordinate.Split(',');

                //string targetCoordinate = comboGridVehicle.CellAt(v, "Target Coordinate").Text;
                //string[] tCor = targetCoordinate.Split(',');

                Point iPoint = _pathfinder.str2coor(comboGridVehicle.CellAt(v, "Current Coordinate").Text);
                //iPoint.X = Convert.ToInt32(iCor[0]);
                //iPoint.Y = Convert.ToInt32(iCor[1]);

                Point tPoint = _pathfinder.str2coor(comboGridVehicle.CellAt(v, "Target Coordinate").Text);
                //tPoint.X = Convert.ToInt32(tCor[0]);
                //tPoint.Y = Convert.ToInt32(tCor[1]);

                if (iPoint.X == -1 || iPoint.Y == -1)
                {
                    return;
                }

                if (tPoint.X == -1 || tPoint.Y == -1)
                {
                    return;
                }

                for (int row = 0; row < Pathfinder.gridSizeY; row++)
                {
                    for (int col = 0; col < Pathfinder.gridSizeX; col++)
                    {
                        if (_pathfinder.Squares[col, row].ContentCode == SquareContent.StartLocation(vehicleID))
                        {
                            _pathfinder.Squares[col, row].ContentCode = SquareContent.FreePath;
                            break;
                        }

                        if (_pathfinder.Squares[col, row].ContentCode == SquareContent.FinishLocation(vehicleID))
                        {
                            _pathfinder.Squares[col, row].ContentCode = SquareContent.FreePath;
                            break;
                        }
                    }
                }

                if (_pathfinder.Squares[iPoint.X, iPoint.Y].ContentCode != SquareContent.Obstacle &&
                    _pathfinder.Squares[tPoint.X, tPoint.Y].ContentCode != SquareContent.Obstacle)
                {
                    _pathfinder.Squares[iPoint.X, iPoint.Y].ContentCode = SquareContent.StartLocation(vehicleID);
                    _pathfinder.Squares[tPoint.X, tPoint.Y].ContentCode = SquareContent.FinishLocation(vehicleID);

                    Recalculate(vehicleID);

                    FillCalculatedPath(vehicleID, false);
                }
            }

            if (errorCount == comboGridVehicle.RowsCount)
            {

                pathCalculated = false;

                //if (errorMessageShowed == false)
                //{
                //    MessageBox.Show("Please check all the parameters, something is wrong !", "Parameter Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
                //    errorMessageShowed = true;
                //}
            }
            else
            {
                pathCalculated = true;
                //btnShowPMList.Enabled = true;
                btnStartSimulationTask.Enabled = true;
            }

            if (onAir && closeConnection && pathCalculated)
            {
                btnStartTask.Enabled = true;
            }

            // sort the agents according to the given criteria 
            if (IsSorted)
            {
                if (byBatteryLevelToolStripMenuItem.Checked)
                {
                    PrioritySort("Battery (%)");
                    IsSorted = true;
                }
                else
                {
                    PrioritySort("Path Length");
                    IsSorted = true;
                }
            }
        }

        public void PrioritySort(string criteria)
        {
            sortedVehicleIDs.Clear();
            sortedVehicleCriteria.Clear();

            for (int rownum = 0; rownum < comboGridVehicle.RowsCount; rownum++)
            {
                if (comboGridVehicle.Titles[rownum].Contains(criteria))
                {
                    if (comboGridVehicle.CellAt(rownum, criteria).Text == null || comboGridVehicle.CellAt(rownum, "Priority").Text == "")
                    {
                        continue;
                    }

                    sortedVehicleIDs.Add(rownum + 1);

                    int pr = Convert.ToInt32(comboGridVehicle.CellAt(rownum, criteria).Text);
                    sortedVehicleCriteria.Add(pr);
                }
                else
                {
 
                }
            }

            int i, j;
            int tmpint;

            for (i = 1; i < sortedVehicleCriteria.Count; i++)
            {
                j = i;
                while (sortedVehicleCriteria[j - 1] < sortedVehicleCriteria[j])
                {
                    tmpint = sortedVehicleCriteria[j - 1];
                    sortedVehicleCriteria[j - 1] = sortedVehicleCriteria[j];
                    sortedVehicleCriteria[j] = tmpint;

                    tmpint = sortedVehicleIDs[j - 1];
                    sortedVehicleIDs[j - 1] = sortedVehicleIDs[j];
                    sortedVehicleIDs[j] = tmpint;

                    j--;
                    if (j == 0) break;
                }
            }

            //foreach (int vID in sortedVehicleIDs)
            //{
            //    MessageBox.Show(vID.ToString());
            //}
        }

        private void btnConnect_Click(object sender, EventArgs e)
        {
            vehicleDrivers = new List<LegoDriver>();

            string nxtName;
            byte[] btAddress;
            int flashMemory;
            int batteryLevel;

            if (closeConnection)
            {
                for (int i = 0; i < comboGridVehicle.RowsCount; i++)
                {

                    if (comboGridVehicle.CellAt(i, "Connection Port").Text == null || comboGridVehicle.CellAt(i, "Connection Port").Text == "")
                    {
                        continue;
                    }

                    if (vehicleDrivers.Count > 0)
                    {
                        vehicleDrivers[i].Disconnect();
                        //vehicleDrivers[i] = null;
                    }

                    closeConnection = false;
                    timerMoveAgents.Enabled = false;
                    btnConnect.Text = "Connect Vehicles";
                }
            }
            else
            {
                for (int i = 0; i < comboGridVehicle.RowsCount; i++)
                {

                    if (comboGridVehicle.CellAt(i, "Connection Port").Text == null || comboGridVehicle.CellAt(i, "Connection Port").Text == "")
                    {
                        continue;
                    }

                    vehicleDrivers.Add(new LegoDriver(comboGridVehicle.CellAt(i, "Connection Port").Text));

                    if (!vehicleDrivers[i].Connect())
                    {
                        string vehicleName = comboGridVehicle.CellAt(i, "Vehicle Name").Text;
                        statusLabel.Text += " Unable to connect " + vehicleName;
                        vehicleDrivers[i] = null;
                    }
                    else
                    {
                        statusLabel.Text = "All vehicles are succesfully connected.";

                        btnConnect.Text = "Disconnect Vehicles";
                        closeConnection = true;

                        if (_capture == null)
                        {
                            startCameraToolStripMenuItem_Click(null, null);
                        }

                        timerMoveAgents.Enabled = true;

                        if (vehicleDrivers[i].GetInformation(out nxtName, out btAddress, out flashMemory, out batteryLevel))
                        {
                            comboGridVehicle.CellAt(i, "Vehicle Name").Items.Add(nxtName);
                            comboGridVehicle.CellAt(i, "Vehicle Name").SelectedIndex = 0;

                            int iBLevel = (int)(batteryLevel / 100);
                            string strBLevel = iBLevel.ToString(); // + " %";

                            comboGridVehicle.CellAt(i, "Battery (%)").Items.Add(strBLevel);
                            comboGridVehicle.CellAt(i, "Battery (%)").SelectedIndex = 0;
                        }
                    }
                }
            }
        }

        private void btnShowPMList_Click(object sender, EventArgs e)
        {
            if (validateGrid(comboGridVehicle, comboGridVehicle.SelectedElement.Y))
            {
                lbPathList.Items.Clear();
                lbMotionList.Items.Clear();

                int vehicleID = comboGridVehicle.SelectedElement.Y + 1;
                FillCalculatedPath(vehicleID, true);
            }
        }        

        /*
        private void CalculateVehicleMovement(int vehicleID)
        {
            List<Point> path = _pathfinder.HighlightPath(vehicleID);
            moves[vehicleID] = new List<Point>();

            for (int i = 1; i < path.Count; i++)
            {
                moves[vehicleID].Add(Pathfinder.ExtractMovement(path[i - 1], path[i]));
            }
        }
        */

        private void btnStartTask_Click(object sender, EventArgs e)
        {
            if (closeConnection && pathCalculated)
            {
                //xold = -1;
                //yold = -1;
                if (byBatteryLevelToolStripMenuItem.Checked)
                {
                    PrioritySort("Battery (%)");
                }
                else
                {
                    PrioritySort("Path Length");
                }

                IsTaskStarted = true;
                timerMoveAgents.Enabled = true;
                btnStartTask.Text = "Stop Task";
            }
            else
            {
                IsTaskStarted = false;
                timerMoveAgents.Enabled = false;

                //for (int i = 0; i < vehicleDrivers.Count; i++)
                //{
                //    StopVehicle(i + 1);
                //}

                btnStartTask.Text = "Start Task";
            }
        }

        void comboGridCell_Handler(object sender, EventArgs ea)
        {
            ComboBox mySelf;
            mySelf = (ComboBox)sender;

            if (mySelf.Text == "Select...")
            {
                //if (CvInvoke.cvCreateCameraCapture(0) != IntPtr.Zero)
                //{
                    ColorRangeForm rangeSelector = new ColorRangeForm();
                    rangeSelector.callerForm = this;
                    rangeSelector.cbCaller = mySelf;

                    if (_capture == null)
                    {
                        startCameraToolStripMenuItem_Click(null, null);
                    }

                    rangeSelector.ShowDialog();
                //}
                //else
                //{
                //    statusLabel.Text = "Please connect a camera!";
                //}
            }

        }

        private void btnObstacle_Click(object sender, EventArgs e)
        {
            //if (CvInvoke.cvCreateCameraCapture(0) != IntPtr.Zero)
            //{
                //tbObstacleColor.Clear();
                ColorRangeForm rangeSelector = new ColorRangeForm();
                rangeSelector.callerForm = this;
                //rangeSelector.tbCaller.Text = obstacleColor;

                if (_capture == null)
                {
                    startCameraToolStripMenuItem_Click(null, null);
                }

                rangeSelector.ShowDialog();


                if (obstacleColor != "")
                {
                    IntRange red = new IntRange(0, 0);
                    IntRange green = new IntRange(0, 0);
                    IntRange blue = new IntRange(0, 0);

                    ParseColorRanges(obstacleColor, red, green, blue);

                    Bitmap obsImg = rangeSelector.img; //CaptureFrame().Bitmap;

                    DetectObstacle(obsImg, red, green, blue);

                    btnConnect.Enabled = true;
                }
            //}
            //else
            //{
            //    statusLabel.Text = "Please connect a camera!";
            //}
        }

        /*
        private void btnGetInitCoor_Click(object sender, EventArgs e)
        {
            if (onAir)
            {
                onAir = false;

                cbLiveCapture.Checked = false;
                timerMoveAgents.Enabled = false;

                btnGetInitCoor.Text = "Stop process";
            }
            else
            {
                try
                {
                    cbLiveCapture.Enabled = true;
                    cbLiveCapture.Checked = false;
                    timerMoveAgents.Enabled = true;
                    onAir = true;
                    btnGetInitCoor.Text = "Get Current Coordinates";
                }
                catch (Exception ex)
                {
                    onAir = false;
                    MessageBox.Show(ex.Message, "Connection Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
                }
            }
        }
        */

        private void timerMoveAgents_Tick(object sender, EventArgs e)
        {
            Image<Bgr, Byte> orgImg = CaptureFrame();
            motherImage = orgImg.Clone();

            if (cbLiveCapture.Checked == true)
            {
                boardControl1.Image = motherImage.Bitmap;
            }
            else
            {
                boardControl1.Image = null;
            }

            int x = -1, y = -1;
            double orientation = -1;
            string xy;

            //foreach (int vehicleID in sortedVehicleIDs)
            for (int row = 0; row < comboGridVehicle.RowsCount; row++)
            {
                int vehicleID;

                if (sortedVehicleCriteria.Count == 0)
                {
                    vehicleID = row + 1;
                }
                else
                {
                    vehicleID = sortedVehicleIDs[row];
                }

                using (Bitmap image = orgImg.Clone().Bitmap)
                {
                    if (comboGridVehicle.CellAt(vehicleID - 1, "Vehicle Name").Text == null || comboGridVehicle.CellAt(vehicleID - 1, "Vehicle Name").Text == "")
                    {
                        continue;
                    }

                    GetVehiclePosition(image, vehicleID, ref x, ref y, ref orientation);

                    if (x > -1 && y > -1)
                    {
                        xy = x.ToString() + "," + y.ToString();

                        if (comboGridVehicle.CellAt(vehicleID - 1, "Initial Coordinate").Text == "")
                        {
                            comboGridVehicle.CellAt(vehicleID - 1, "Initial Coordinate").Text = xy;
                        }

                        comboGridVehicle.CellAt(vehicleID - 1, "Current Coordinate").Text = xy;
                        comboGridVehicle.CellAt(vehicleID - 1, "Orientation Angle").Text = orientation.ToString();

                        //Recalculate(row + 1);
                        if (comboGridVehicle.CellAt(vehicleID - 1, "Target Coordinate").Text != "")
                        {
                            btnCalculate_Click(null, null);
                        }
                    }

                    if (IsTaskStarted)
                    {
                        ControlVehicle(vehicleID);
                    }
                }
            }
        }

        private int simulateTick;
        private int totalPathCount;

        private void btnStartSimulationTask_Click(object sender, EventArgs e)
        {
            if (runSimulation == true)
            {
                btnStartSimulationTask.Text = "Start Simulation Task";
                timerSimulateAgents.Enabled = false;
                runSimulation = false;
            }
            else
            {
                for (int rownum = 0; rownum < comboGridVehicle.RowsCount; rownum++)
                {
                    if (comboGridVehicle.CellAt(rownum, "Path Length").Text == null ||
                        comboGridVehicle.CellAt(rownum, "Path Length").Text == "")
                    {
                        continue;
                    }

                    sortedVehicleIDs.Add(rownum + 1);

                    int pr;
                    pr = Convert.ToInt32(comboGridVehicle.CellAt(rownum, "Path Length").Text);
                    
                    totalPathCount = totalPathCount + pr;
                    
                    sortedVehicleCriteria.Add(pr);
                }

                btnStartSimulationTask.Text = "Stop Simulation Task";

                simulateTick = 0;

                timerSimulateAgents.Enabled = true;
                runSimulation = true;
            }
        }

        private void timerSimulateAgents_Tick(object sender, EventArgs e)
        {
            foreach (int vehicleID in sortedVehicleIDs)
            {
                Point myStep;

                if (vehicleStepCounters[vehicleID] < paths[vehicleID].Count)
                {
                    myStep = paths[vehicleID][vehicleStepCounters[vehicleID]];
                    vehicleStepCounters[vehicleID] += 1;

                    ModifyInitialPath(vehicleID, myStep.X, myStep.Y);
                    _pathfinder.DrawBoard(boardControl1, vehicleID, comboGridVehicle, obstacleColor);
                    boardControl1.Invalidate();
                }
                
            }

            simulateTick += simulateTick;

            //int taskDoneCounter = 0;
            //int vehicleCounter = 0;

            //for (int i = 0; i < comboGridVehicle.RowsCount; i++)
            //{
            //    if (comboGridVehicle.CellAt(i, "Task Status").Text == "Done !")
            //    {
            //        taskDoneCounter += taskDoneCounter;
            //    }

            //    if (comboGridVehicle.CellAt(i, "Target Coordinate").Text != "" &&
            //        comboGridVehicle.CellAt(i, "Current Coordinate").Text != "" &&
            //        comboGridVehicle.CellAt(i, "Initial Coordinate").Text != "")
            //    {
            //        vehicleCounter += vehicleCounter;
            //    }
            //}

            if (totalPathCount == simulateTick)
            {
                btnStartSimulationTask.Text = "Start Simulation Task";
                timerSimulateAgents.Enabled = false;
                runSimulation = false;
                btnCalculate_Click(null, null);
            }
        }

        private void bluetoothPropertiesToolStripMenuItem_Click(object sender, EventArgs e)
        {
            //string bthprops = "C:\\WINDOWS\\System32\\bthprops.cpl"; // Windows Vista
            string btcpl = "C:\\WINDOWS\\System32\\btcpl.cpl"; // Windows XP

            try
            {
                //System.Diagnostics.Process.Start(bthprops);
                System.Diagnostics.Process.Start(btcpl);
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message, "Bluetooth Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
                System.Diagnostics.Process.Start("control");
            }
        }

        private void exitToolStripMenuItem_Click(object sender, EventArgs e)
        {
            this.Close();
        }

        private void startCameraToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (startCameraToolStripMenuItem.Checked == false)
            {
                try
                {
                    CameraSelection cs = new CameraSelection();
                    cs.ShowDialog();

                    if (cs.cbCameraList.Text != "" || cs.cbCameraList.Text != "Please connect a camera")
                    {
                        _capture = new Capture(cs.cbCameraList.SelectedIndex);

                        startCameraToolStripMenuItem.Checked = true;
                        startCameraToolStripMenuItem.Text = "Stop Camera";
                    }
                }
                catch (Exception ex)
                {
                    MessageBox.Show(ex.Message, "Connection Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
                    startCameraToolStripMenuItem.Checked = false;
                    startCameraToolStripMenuItem.Text = "Start Camera";
                }
            }
            else
            {
                _capture.Dispose();
                _capture = null;
                
                startCameraToolStripMenuItem.Checked = false;
                startCameraToolStripMenuItem.Text = "Start Camera";
            }
        }

        private void vehicleParametersToolStripMenuItem_Click(object sender, EventArgs e)
        {
            VehicleParameters vp = new VehicleParameters();
            vp.mf = this;
            vp.ShowDialog();
        }

        private void connectVehiclesToolStripMenuItem_Click(object sender, EventArgs e)
        {
            btnConnect_Click(null, null);
        }

        private void startLiveCaptureToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (cbLiveCapture.Checked == false)
            {
                cbLiveCapture.Checked = true;
                startLiveCaptureToolStripMenuItem.Text = "Stop Live Capture";
            }
            else
            {
                cbLiveCapture.Checked = false;
                startLiveCaptureToolStripMenuItem.Text = "Start Live Capture";
            }
        }

        private void calculatePathsToolStripMenuItem_Click(object sender, EventArgs e)
        {
            btnCalculate_Click(null, null);
        }

        private void startTaskToolStripMenuItem_Click(object sender, EventArgs e)
        {
            btnStartTask_Click(null, null);
        }

        private int selectedVehicle;

        private void cbSetManualVehicleControl_Click(object sender, EventArgs e)
        {
            selectedVehicle = comboGridVehicle.SelectedElement.Y + 1;

            if (closeConnection)
            {
                
                btnGoForward.Enabled = cbSetManualVehicleControl.Checked;
                btnGoBackward.Enabled = cbSetManualVehicleControl.Checked;
                btnGoLeft.Enabled = cbSetManualVehicleControl.Checked;
                btnGoRight.Enabled = cbSetManualVehicleControl.Checked;
                btnStopGo.Enabled = cbSetManualVehicleControl.Checked;
                btnTurnCCW.Enabled = cbSetManualVehicleControl.Checked;
                btnTurnCW.Enabled = cbSetManualVehicleControl.Checked;

                StopVehicle(selectedVehicle);
            }
            else
            {
                cbSetManualVehicleControl.Checked = false;
                cbSetManualVehicleControl.CheckState = CheckState.Indeterminate;
            }
        }

        private void btnStopGo_Click(object sender, EventArgs e)
        {
            StopVehicle(selectedVehicle);
        }

        private void btnGoForward_Click(object sender, EventArgs e)
        {
            MarchVehicle(selectedVehicle);
        }

        private void btnGoBackward_Click(object sender, EventArgs e)
        {
            vehicleDrivers[selectedVehicle - 1].SetMotors(leftMotor, rightMotor, -leftMotorPower, -rightMotorPower, 50, 0);
        }

        private void btnGoLeft_Click(object sender, EventArgs e)
        {
            TurnVehicle(selectedVehicle,true);
        }

        private void btnTurnCW_Click(object sender, EventArgs e)
        {
            vehicleDrivers[selectedVehicle - 1].SetMotors(leftMotor, rightMotor, leftMotorPower, -rightMotorPower, 100, 250);
        }

        private void btnGoRight_Click(object sender, EventArgs e)
        {
            TurnVehicle(selectedVehicle, false);
        }

        private void btnTurnCCW_Click(object sender, EventArgs e)
        {
            vehicleDrivers[selectedVehicle - 1].SetMotors(leftMotor, rightMotor, -leftMotorPower, rightMotorPower, 100, 250);
        }

        private void cbGetVehicleInformation_Click(object sender, EventArgs e)
        {
            selectedVehicle = comboGridVehicle.SelectedElement.Y + 1;

            if (closeConnection)
            {
                tbVehicleName.Enabled = cbGetVehicleInformation.Checked;
                btnChangeVehicleName.Enabled = cbGetVehicleInformation.Checked;

                string vehicleName;
                byte[] btAddress;
                int flashMemory;
                int batteryLevel;

                if (vehicleDrivers[selectedVehicle - 1].GetInformation(out vehicleName, out btAddress, out flashMemory, out batteryLevel))
                {
                    tbVehicleName.Text = vehicleName;
                    tbBTAdress.Text = string.Format("{0} {1} {2} {3} {4} {5} {6}",
                    btAddress[0].ToString("X2"),
                    btAddress[1].ToString("X2"),
                    btAddress[2].ToString("X2"),
                    btAddress[3].ToString("X2"),
                    btAddress[4].ToString("X2"),
                    btAddress[5].ToString("X2"),
                    btAddress[6].ToString("X2")
                    );

                    int iFMemory = (int)(flashMemory / 1000);
                    string strFMemory = iFMemory.ToString() + " %";

                    toolTip1.SetToolTip(pbFlashMemory, strFMemory);
                    pbFlashMemory.Value = iFMemory;

                    int iBLevel = (int)(batteryLevel / 100);
                    string strBLevel = iBLevel.ToString() + " %";

                    toolTip1.SetToolTip(pbBatteryLevel, strBLevel);
                    pbBatteryLevel.Value = iBLevel;
                }
            }
            else
            {
                cbGetVehicleInformation.Checked = false;
                cbGetVehicleInformation.CheckState = CheckState.Indeterminate;
            }
        }

        private void btnChangeVehicleName_Click(object sender, EventArgs e)
        {
            if (tbVehicleName.Text != null && tbVehicleName.Text != "" && closeConnection)
            {
                if (vehicleDrivers[selectedVehicle - 1].SetDeviceName(tbVehicleName.Text))
                {
                    comboGridVehicle.CellAt(selectedVehicle - 1, "Vehicle Name").Items.Clear();
                    comboGridVehicle.CellAt(selectedVehicle - 1, "Vehicle Name").Items.Add(tbVehicleName.Text);
                    comboGridVehicle.CellAt(selectedVehicle - 1, "Vehicle Name").SelectedIndex = 0;
                }
            }
        }

        private void cbSetManualVehicleControl_KeyDown(object sender, KeyEventArgs e)
        {
            if (e.KeyData.ToString() == "W")
                btnGoForward_Click(null, null);
            if (e.KeyData.ToString() == "S")
                btnGoBackward_Click(null, null);
            if (e.KeyData.ToString() == "A")
                btnGoLeft_Click(null, null);
            if (e.KeyData.ToString() == "D")
                btnGoRight_Click(null, null);
            if (e.KeyData.ToString() == "Q")
                btnTurnCW_Click(null, null);
            if (e.KeyData.ToString() == "E")
                btnTurnCCW_Click(null, null);
            if (e.KeyData.ToString() == "Escape")
                btnStopGo_Click(null, null);
        }

    }
}